﻿#define PRECISE
using System;
using System.Runtime.CompilerServices;
using RayDen.Library.Core.Primitives;

namespace RayDen.Library.Core
{
    public static class MathLab
    {
        public static float M_PI = 3.14159265358979323846f;
        public static float M_2PI = 3.14159265358979323846f * 2f;
        public static float INVPI = 0.31830988618379067154f;
        public static float INVTWOPI = 0.15915494309189533577f;
        public static float INV4PI = 0.07957747154594766788450883936693f;

        public static float OneThird = 1f / 3f;
        public static float Epsilon = 0.000001f;
        public static float RAY_EPSILON = 0.001f;
        public static float COS_EPSILON_STATIC = 1e-4f;
        public static float SpectreEpsilon = 0.000001f;

        public static bool NearEqual(this float f, float arg, float eps = 0.00001f)
        {
            return
                Math.Abs(f - arg) <= eps;
        }

        public static bool NearZero(this float t, float eps = 0.000001f)
        {
            return t > -eps & t < eps;
        }

        public static float PdfWtoA(float aPdfW, float aDist, float aCosThere)
        {
            return aPdfW * Math.Abs(aCosThere) / (aDist * aDist);
        }

        public static float PdfAtoW(
             float aPdfA,
             float aDist,
             float aCosThere)
        {
            return aPdfA * (aDist * aDist) / Math.Abs(aCosThere);
        }
        public static float SphericalTheta(ref Vector v)
        {
            return (float)Math.Acos(Clamp(v.z, -1f, 1f));
        }

        public static float SphericalPhi(ref Vector v)
        {
            return (float)Math.Atan2(v.y, v.x) + M_PI;
        }

        public static float CosTheta(Vector w) { return w.z; }
        public static float AbsSinTheta(Vector w)
        {
            return MathLab.Sqrt(Math.Max(0f, 1f - w.z * w.z));
        }

        /// <summary>
        /// Fast Power approximation
        /// </summary>
        /// <param name="a"> power </param>
        /// <param name="b"> number </param>
        /// <returns></returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static double Pow(double a, double b)
        {
            var tmp = (int)(BitConverter.DoubleToInt64Bits(a) >> 32);
            var tmp2 = (int)(b * (tmp - 1072632447) + 1072632447);
            return (BitConverter.Int64BitsToDouble(((long)tmp2) << 32));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float FastSqrt(float value)
        {
            var u = new Int32Single { f = value };
            u.i = (1 << 29) + (u.i >> 1) - (1 << 22) - 0x4C000;
            return u.f;
        }
        public static Vector SphericalDirection(float sintheta, float costheta, float phi)
        {
            return new Vector(sintheta * (float)Math.Cos(phi), sintheta * (float)Math.Sin(phi), costheta);
        }

        public static Vector SphericalDirection(float sintheta, float costheta, float phi,
                                                ref Vector x, ref Vector y, ref Vector z)
        {
            return sintheta * (float)Math.Cos(phi) * x + sintheta * (float)Math.Sin(phi) * y + costheta * z;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Clamp(float cosi, float p, float p_3)
        {
            if (cosi < p)
                return p;
            if (cosi > p_3)
                return p_3;
            return cosi;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static double Clamp(double cosi, double p, double p_3)
        {
            if (cosi < p)
                return p;
            if (cosi > p_3)
                return p_3;
            return cosi;
        }
        public static int Clamp(int cosi, int p, int p_3)
        {
            int r = cosi;
            if (r < p)
                r = p;
            if (r > p_3)
                r = p_3;

            return r;

        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static bool NearZero(float value)
        {
            return value > 0f & value < MathLab.Epsilon;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static void Swap(ref float e0, ref float et)
        {
            float tmp = e0;
            e0 = et;
            et = tmp;
        }

        public static float Radians(float angle)
        {
            return (M_PI / 180f) * angle;
        }

        /*
     double molif_r = 1d; // Global mollification radius, shrinks per sample
     double mollify(Vec& l, cVec& rd, Vec& n, Vec& nl, double dist, int type){
double cos_max=1./sqrt(1.+(molif_r/dist)*(molif_r/dist));// Cone angle
double solid_angle=2.*M_PI*(1.-cos_max); // Solid angle of the cone
Vec out = rd -n*2*n.dot(rd); // Reflection vector
if(type == REFR){ // Compute refraction vector
double nc=1,nt=1.5,nnt=n.dot(nl)>0?nc/nt:nt/nc,ddn=rd.dot(nl),cos2t;
if((cos2t=1-nnt*nnt*(1-ddn*ddn))>0) // Refraction vector
out =(rd*nnt-n*((n.dot(nl)>0?1:-1)*(ddn*nnt+sqrt(cos2t)))).norm();
}
return l.dot(out)>=cos_max?(1./solid_angle)/l.dot(out):0.; // Mollify
}
             */

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float MolifyReflection(float molif_r, ref Vector l, ref Vector rd, ref Normal n, ref Normal nl, float dist)
        {
            var cos_max = 1f / Sqrt(1f + (molif_r / dist) * (molif_r / dist));// Cone angle
            var solid_angle = 2f * M_PI * (1f - cos_max); // Solid angle of the cone
            var outv = rd - (n * 2 * n.Dot(ref rd)).ToVec(); // Reflection vector
            return l.Dot(ref outv) >= cos_max ? (1f / solid_angle) / l.Dot(ref outv) : 0f; // Mollify
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float MolifyRefraction(float molif_r, ref Vector l, ref Vector rd, ref Normal n, ref Normal nl, float dist, float nc = 1f, float nt = 1.5f)
        {
            var cos_max = 1f / Sqrt(1f + (molif_r / dist) * (molif_r / dist));// Cone angle
            var solid_angle = 2f * M_PI * (1f - cos_max); // Solid angle of the cone
            var outv = rd - (n * 2 * n.Dot(ref rd)).ToVec(); // Reflection vector


            float nnt = n.Dot(ref nl) > 0 ? nc / nt : nt / nc, ddn = rd.Dot(ref nl), cos2t;
            if ((cos2t = 1 - nnt * nnt * (1 - ddn * ddn)) > 0) // Refraction vector
                outv = (rd * nnt - (n * ((n.Dot(ref nl) > 0 ? 1 : -1) * (ddn * nnt + Sqrt(cos2t)))).ToVec()).Normalize();

            return l.Dot(ref outv) >= cos_max ? (1f / solid_angle) / l.Dot(ref outv) : 0f; // Mollify
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float AngularRegularization(ref Vector w0, ref Vector wi, float epsilon)
        {
            float ind = Vector.Dot(ref w0, ref wi) < epsilon ? 1f : 0f;
            return (1f / (M_2PI * (1f - Cos(epsilon)))) * ind;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float Sqrt(float p)
        {
#if PRECISE
            return (float)Math.Sqrt(p);
#else
            return FastSqrt(p);
#endif
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float AbsDot(ref Vector wo, ref Vector wh)
        {
            return Math.Abs(Vector.Dot(ref wo, ref wh));
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static int Floor2UInt(float val)
        {
            return val > 0 ? (int)(val) : 0;
        }
        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static int Ceil2UInt(float val)
        {
            return val > 0f ? (int)Math.Ceiling(val) : 0;
        }
        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float Lerp(float t, float v1, float v2)
        {
            return (1f - t) * v1 + t * v2;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float DistanceSquared(ref Vector p1, ref Vector p2)
        {
            return (p1 - p2).Length2();
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float DistanceSquared(ref Point p1, ref Point p2)
        {
            return (p1 - p2).Length2();
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float Cos(float r1)
        {
            return (float)Math.Cos(r1);
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float Sin(float r1)
        {
            return (float)Math.Sin(r1);
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static float Exp(float p)
        {
            return (float)Math.Exp(p);
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static Vector Exp(Vector p)
        {
            return new Vector((float)Math.Exp(p.x), (float)Math.Exp(p.y), (float)Math.Exp(p.z));
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static int Float2Int(float p)
        {
            return (int)Math.Round(p);
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static bool SmallEnough(float p1, float p2)
        {
            return Math.Abs(p2 - p1) < Epsilon;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static int Floor(float d)
        {
            //return (int) Math.Floor(d);
            return (int.MaxValue + (int)((d - int.MaxValue) - 1));
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public static bool SolveLinearSystem2X2(float[][] A, float[] B, out float x, out float y)
        {
            x = float.NaN;
            y = float.NaN;
            float det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
            if (Math.Abs(det) < 1e-5)
                return false;
            float invDet = 1.0f / det;
            x = (A[1][1] * B[0] - A[0][1] * B[1]) * invDet;
            y = (A[0][0] * B[1] - A[1][0] * B[0]) * invDet;
            return true; 

        }

    }
}

